/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "voslam_bookkeep.h"

/* gcc -O2 -Wall -g -c -o voslam_bookkeep.o voslam_bookkeep.c */

void parse_matches(
		double *fea,
		int *indA,
		int *indB,
		int *mask,
		int start,
		int stop,
		int *N
	     )
{
    int i,k,D=10,cnt=0;

    k=0;
    printf("fea start %d stop %d\n",start,stop);
    for(i=start;i<stop;i++)
    {
        if(fea[D*i+2]>=0)	/* next */
        {
            cnt++;
            indA[k]=i;
            indB[k]=(int)fea[D*i+2]; /* next */
            if(fea[D*i+6]==-1)	/* map */
                mask[k]=0;
            else
                mask[k]=(int)fea[D*i+6];	/* map */
            k++;
        }
    }
     printf("%d fea parsed\n",cnt);
    *N=k;
}

void slam_parse_matches(
        double *fea,
        int *indA,
        int *indB,
        int camera,
        int *mask,
        int start,
        int stop,
        int *N
         )
{
    int i,k,D=14,cnt=0;

    k=0;
    printf("fea start %d stop %d\n",start,stop);
    for(i=start;i<stop;i++)
    {
        if(fea[D*i+2]>=0)   /* next */
        {
            if(fea[D*i+4]==camera)
            {
                cnt++;
                indA[k]=i;
                indB[k]=(int)fea[D*i+2]; /* next */
                if(fea[D*i+6]==-1)  /* map */
                    mask[k]=0;
                else
                    mask[k]=(int)fea[D*i+6];    /* map */
                k++;
            }
        }
    }
     printf("%d fea parsed\n",cnt);
    *N=k;
}
void slam_stereo_parse_matches(
        double *fea,
        int *indA,
        int *indB,
        int *mask,
        int start,
        int stop,
        int *N
         )
{
    int i,k,D=14,cnt=0;

    k=0;
    printf("fea start %d stop %d\n",start,stop);
    for(i=start;i<stop;i++)
    {
        if(fea[D*i+7]>=0)   /* other */
        {
            if(fea[D*i+4]==0)
            {
            cnt++;
            indA[k]=i;
            indB[k]=(int)fea[D*i+7]; /* other */
            if(fea[D*i+6]==-1)  /* map */
                mask[k]=0;
            else
                mask[k]=(int)fea[D*i+6];    /* map */
            k++;
            }
        }
    }
     printf("%d stereo fea parsed\n",cnt);
    *N=k;
}
void update_fea(
		double *fea,
		double *featuresB,
		int frame,
		int *start,
		int num_featuresA,
		int num_featuresB,
		int *matches,
        float *strength
	     )
{
    int i,j,ind,col,D=10,cnt=0; 
    printf("frame %d\n",frame);
    start[frame]=start[frame-1]+num_featuresB;
    printf("num_featuresB %d\n",num_featuresB);
    for(i=0;i<num_featuresB;i++)
    {
       j=start[frame-1]+i;
       col=D*j;
       fea[col]=featuresB[3*i];	    /* x */
       fea[1+col]=featuresB[3*i+1];   	/* y */
       fea[2+col]=-1;			/* next */
       fea[3+col]=-1;			/* prev */
       fea[4+col]=featuresB[3*i+2];		/* cor */
       fea[5+col]=frame;		/* frame */
       fea[6+col]=-1;			/* map */
       fea[7+col]=strength[2*i];           /* maxval */
       fea[8+col]=strength[2*i+1];         /* maaxval2 */
       fea[9+col]=-1;           /* ? */

    }
    printf("num_featuresA %d\n",num_featuresA);
    for(i=0;i<num_featuresA;i++)	
    {
        ind=matches[i];
        if(ind>0)
        {
            cnt++;
            ind--;

            fea[2+(start[frame-2]+i)*D]=start[frame-1]+ind;      /* next*/
            fea[3+(start[frame-1]+ind)*D]=start[frame-2]+i;      /* prev */
            fea[6+D*(start[frame-1]+ind)]=fea[6+D*(start[frame-2]+i)];	/* map */
        }
    }
    printf("%d fea updated\n",cnt);
}


void stereo_update_fea(
        double *fea,
        double *featuresB,
        int frame,
        int camera,
        int *start,
        int num_featuresA,
        int num_featuresB,
        int *matches
         )
{
    int i,j,ind,col,D=14,cnt=0; 
    printf("frame %d\n",frame);
    start[frame]=start[frame-1]+num_featuresB;
    printf("num_featuresB %d\n",num_featuresB);
    for(i=0;i<num_featuresB;i++)
    {
       j=start[frame-1]+i;
       col=D*j;
       fea[col]=featuresB[3*i];     /* x */
       fea[1+col]=featuresB[3*i+1];     /* y */
       fea[2+col]=-1;           /* next */
       fea[3+col]=-1;           /* prev */
       fea[4+col]=camera;           /* map */
       fea[5+col]=frame;        /* frame */
       fea[6+col]=-1;           /* map */
       fea[7+col]=-1;           /* other */
    }
    printf("num_featuresA %d\n",num_featuresA);
    for(i=0;i<num_featuresA;i++)    
    {
        ind=matches[i];
        if(ind>0)
        {
            cnt++;
            ind--;

            fea[2+(start[frame-2]+i)*D]=start[frame-1]+ind;      /* next*/
            fea[3+(start[frame-1]+ind)*D]=start[frame-2]+i;      /* prev */
            fea[6+D*(start[frame-1]+ind)]=fea[6+D*(start[frame-2]+i)];  /* map */
        }
    }
    printf("%d fea updated\n",cnt);
}

void slam_inc_XX(    double *XX,
        double *fea,
        double *X,
        int *indB,
        int *inumm,
        int num_featuresB
         )
{
    int i,numm,indi,D=14;
    
    numm=*inumm;    
    for(i=0;i<num_featuresB;i++)
    {
        XX[3*(numm-1)]=X[3*i];
        XX[3*(numm-1)+1]=X[3*i+1];
        XX[3*(numm-1)+2]=X[3*i+2];
        indi=indB[i];
        fea[6+indi*D]=numm;     /* map */
        if(indi<0)
        {
            printf("error: slam_inc_XX indi is %d\n",indi);
            exit(1);
        }
        indi=(int)fea[3+indi*D];    /* prev */
        fea[6+indi*D]=numm; /* map */
        numm++;
    }
    *inumm=numm;
}
void slam_stereo_inc_XX(    double *XX,
        double *fea,
        double *X,
        int *indB,
        int *inumm,
        int num_featuresB
         )
{
    int i,numm,indi,D=14;
    
    numm=*inumm;    
    for(i=0;i<num_featuresB;i++)
    {
        XX[3*(numm-1)]=X[3*i];
        XX[3*(numm-1)+1]=X[3*i+1];
        XX[3*(numm-1)+2]=X[3*i+2];
        indi=indB[i];
        if(indi<0)
        {
            printf("error: slam_stereo_inc_XX indi is %d\n",indi);
            exit(1);
        }
        if((int)fea[7+indi*D]>=0)
        {
            fea[6+indi*D]=numm;     /* map */
            indi=(int)fea[7+indi*D];    /* other */
            fea[6+indi*D]=numm; /* map */
            numm++;
        }
    }
    *inumm=numm;
}

void inc_XX(    double *XX,
		double *fea,
		double *X,
		int *indB,
		int *inumm,
		int num_featuresB
	     )
{
    int i,numm,indi,D=10;
    
    numm=*inumm;    
    for(i=0;i<num_featuresB;i++)
    {
        XX[3*(numm-1)]=X[3*i];
        XX[3*(numm-1)+1]=X[3*i+1];
        XX[3*(numm-1)+2]=X[3*i+2];
        indi=indB[i];
        fea[6+indi*D]=numm; 	/* map */
        indi=(int)fea[3+indi*D];	/* prev */
        fea[6+indi*D]=numm;	/* map */
        numm++;
    }
    *inumm=numm;
}

void update_XX(    double *XX,
		double *X,
		int *indX,
		int N
	     )
{
    int i,numm;
    
       
    for(i=0;i<N;i++)
    {
        numm=indX[i]-1;
        XX[3*numm]=X[3*i];
        XX[3*numm+1]=X[3*i+1];
        XX[3*numm+2]=X[3*i+2];
    }
}
void parse_fea_for_slam(
        double *fea,
        double *XX,
        double *z,
        int *ind,
        int *indf,
        double *X,
        int *indx,
        int *indX,
        int *indz,
        int *jj,
        int *kk,
        int N,
        int window
 )
{
    int i,j,k,numm,frame,D=10,maxframe,minframe;
    
    maxframe=fea[D*(N-1)+5];
    minframe=maxframe-window-1;
    if(minframe<0)
        minframe=0;

    for(i=0;i<N;i++)
        indx[i]=0;
    j=0;
    k=0;
    for(i=0;i<N;i++)
    {
        numm=fea[D*i+6];
        frame=fea[D*i+5]-minframe;
        if(frame>0)
        {
            if(numm>0)
            {
                indf[j]=frame-1;
                indz[j]=i;
                z[2*j]=fea[D*i];
                z[2*j+1]=fea[D*i+1];
    
                if(indx[numm-1]==0)
                {
                    X[3*k]=XX[3*(numm-1)];
                    X[3*k+1]=XX[3*(numm-1)+1];
                    X[3*k+2]=XX[3*(numm-1)+2];
                    indx[numm-1]=k+1;
                    indX[k]=numm;
                    k++;
                }
                ind[j]=indx[numm-1];
                j++;
            }
        }
    }
    *kk=k;
    *jj=j;
}

void stereo_parse_args_for_slam(
        double *fea,
        double *XX,
        double *z,
        int *ind,
        int *indf,
        int *indc,
        double *X,
        int *indx,
        int *indX,
        int *indz,
        int *jj,
        int *kk,
        int N,
        int window
 )
{
    int i,j,k,l,numm,D=14,maxframe,minframe;
    
    maxframe=fea[D*(N-1)+5];
    minframe=maxframe-window+1;
    if(minframe<2)
        minframe=2;

    /*
        if((frame-window)<1)
            rwindow=frame;
        else
        {
            rwindow=window;

        }
        printf("rwindow:%d %d\n",frame,rwindow);
        printf("the_frame: %d %d\n",frame,num);
        D=rwindow*statedim+kk*3;
        printf("D: %d\n",D);
        for(i=0;i<D;i++)
            X[i]=Xpos[i+(frame+1-rwindow)*statedim];
        num_frames=rwindow+1;*/
    for(i=0;i<N;i++)
        indx[i]=0;
    j=0;
    k=0;
    for(i=N-1;i>0;i--)
    {

      /* find last node of a track */     
        if((int)fea[D*i+5]<minframe)
            break;
        if((fea[2+D*i]==-1)&&(fea[3+D*i]!=-1)&&(fea[6+D*i]!=-1))
        {
            l=i;
            numm=fea[D*i+6];
            
            if(indx[numm-1]==0)
            {
                X[3*k]=XX[3*(numm-1)];
                X[3*k+1]=XX[3*(numm-1)+1];
                X[3*k+2]=XX[3*(numm-1)+2];
                indx[numm-1]=k+1;
                indX[k]=numm;
                k++;
            }
            /* follow track to first node */
            while(fea[D*l+3]>-1)
            {
                if(fea[D*l+5]<minframe)
                {
                    indf[j]=-fea[D*l+5]+1;
                }
                else
                {
                    indf[j]= fea[D*l+5]-minframe+1;
                }
                indz[j]=l;
                z[2*j]=fea[D*l];
                z[2*j+1]=fea[D*l+1];
                indc[j]=fea[D*l+4];
                  
                ind[j]=indx[numm-1];
                j++; 
                l=fea[D*l+3];
            }
            if(fea[D*l+5]<minframe)
            {
                indf[j]=-fea[D*l+5]+1;
            }
            else
            {
                indf[j]= fea[D*l+5]-minframe+1;
            }
            indz[j]=l;
            z[2*j]=fea[D*l];
            z[2*j+1]=fea[D*l+1];
            indc[j]=fea[D*l+4];
            
            ind[j]=indx[numm-1];
            j++; 
             
        }
        
   /*     if((fea[2+D*i]==-1)&&(fea[3+D*i]==-1)&&(fea[6+D*i]!=-1))
        {
            if(fea[7+D*i]>0)
            {
                numm=fea[D*i+6];
            
                if(indx[numm-1]==0)
                {
                    X[3*k]=XX[3*(numm-1)];
                    X[3*k+1]=XX[3*(numm-1)+1];
                    X[3*k+2]=XX[3*(numm-1)+2];
                    indx[numm-1]=k+1;
                    indX[k]=numm;
                    k++;
                }
                indz[j]=i;
                z[2*j]=fea[D*i];
                z[2*j+1]=fea[D*i+1];
                indc[j]=fea[D*i+4];
                indf[j]= fea[D*i+5]-minframe+1;
                ind[j]=indx[numm-1];
                j++; 
            }
        }*/
    }
    *kk=k;
    *jj=j;

}

void initial_stereo_parse_args_for_slam(
        double *fea,
        double *XX,
        double *z,
        int *ind,
        int *indf,
        int *indc,
        double *X,
        int *indx,
        int *indX,
        int *indz,
        int *jj,
        int *kk,
        int N
 )
{
    int i,j,k,numm,D=14;
    /*
        if((frame-window)<1)
            rwindow=frame;
        else
        {
            rwindow=window;

        }
        printf("rwindow:%d %d\n",frame,rwindow);
        printf("the_frame: %d %d\n",frame,num);
        D=rwindow*statedim+kk*3;
        printf("D: %d\n",D);
        for(i=0;i<D;i++)
            X[i]=Xpos[i+(frame+1-rwindow)*statedim];
        num_frames=rwindow+1;*/
    for(i=0;i<N;i++)
        indx[i]=0;
    j=0;
    k=0;
    for(i=N-1;i>0;i--)
    {

      /* find last node of a track */     

        if((fea[2+D*i]==-1)&&(fea[3+D*i]==-1))
        {
            if(fea[7+D*i]>0)
            {
                numm=fea[D*i+6];
            
                if(indx[numm-1]==0)
                {
                    X[3*k]=XX[3*(numm-1)];
                    X[3*k+1]=XX[3*(numm-1)+1];
                    X[3*k+2]=XX[3*(numm-1)+2];
                    indx[numm-1]=k+1;
                    indX[k]=numm;
                    k++;
                }
                indz[j]=i;
                z[2*j]=fea[D*i];
                z[2*j+1]=fea[D*i+1];
                indc[j]=fea[D*i+4];
                indf[j]=-fea[D*i+5]+1;
                ind[j]=indx[numm-1];
                j++; 
            }
        }
    }
    *kk=k;
    *jj=j;

}

void parse_args_for_slam(
        double *fea,
        double *XX,
        double *z,
        int *ind,
        int *indf,
        double *X,
        int *indx,
        int *indX,
        int *indz,
        int *jj,
        int *kk,
        int N,
        int window
 )
{
    int i,j,k,l,numm,D=10,maxframe,minframe;
    
    maxframe=fea[D*(N-1)+5];
    minframe=maxframe-window+1;
    if(minframe<2)
        minframe=2;

    /*
        if((frame-window)<1)
            rwindow=frame;
        else
        {
            rwindow=window;

        }
        printf("rwindow:%d %d\n",frame,rwindow);
        printf("the_frame: %d %d\n",frame,num);
        D=rwindow*statedim+kk*3;
        printf("D: %d\n",D);
        for(i=0;i<D;i++)
            X[i]=Xpos[i+(frame+1-rwindow)*statedim];
        num_frames=rwindow+1;*/
    for(i=0;i<N;i++)
        indx[i]=0;
    j=0;
    k=0;
    for(i=N-1;i>0;i--)
    {

      /* find last node of a track */     
        if((int)fea[D*i+5]<minframe)
            break;
        if((fea[2+D*i]==-1)&&(fea[3+D*i]!=-1)&&(fea[6+D*i]!=-1))
        {
            l=i;
            numm=fea[D*i+6];
            
            if(indx[numm-1]==0)
            {
                X[3*k]=XX[3*(numm-1)];
                X[3*k+1]=XX[3*(numm-1)+1];
                X[3*k+2]=XX[3*(numm-1)+2];
                indx[numm-1]=k+1;
                indX[k]=numm;
                k++;
            }
            /* follow track to first node */
            while(fea[D*l+3]>-1)
            {
                if(fea[D*l+5]<minframe)
                {
                    indf[j]=-fea[D*l+5]+1;
                }
                else
                {
                    indf[j]= fea[D*l+5]-minframe+1;
                }
                indz[j]=l;
                z[2*j]=fea[D*l];
                z[2*j+1]=fea[D*l+1];
                  
                ind[j]=indx[numm-1];
                j++; 
                l=fea[D*l+3];
            }
            if(fea[D*l+5]<minframe)
            {
                indf[j]=-fea[D*l+5]+1;
            }
            else
            {
                indf[j]= fea[D*l+5]-minframe+1;
            }
            indz[j]=l;
            z[2*j]=fea[D*l];
            z[2*j+1]=fea[D*l+1];
                  
            ind[j]=indx[numm-1];
            j++; 
             
        }
    }
    *kk=k;
    *jj=j;

}

void prune_fea(
		double *fea,
		int *ind2,
		int *mask,
		int N
	     )
{
    int i,index1,index2,i2,D=10; 
    

    for(i=0;i<N;i++)	
    {
        
        if(mask[i]<0)
        {
           
            i2=ind2[i];
            index1=fea[2+i2*D];
            index2=fea[3+i2*D];
            fea[2+i2*D]=-1;
            fea[3+i2*D]=-1;
            fea[6+i2*D]=-1;
            if(index2>=0)
            {
                fea[2+index2*D]=-1;
                if(fea[3+index2*D]==-1)
                    fea[6+index2*D]=-1;
            }
        
            if(index1>=0)
            {
                fea[3+index1*D]=-1;
                if(fea[2+index1*D]==-1)
                    fea[6+index1*D]=-1;
            }
        }
    }
}

void prune_slamfea(
        double *fea,
        int *indz,
        double *er,
        double th,
        int N
         )
{
    int i,index1,index2,i2,D=10; 
    

    for(i=0;i<N;i++)    
    {
        
        if(er[i]>th)
        {
           
            i2=indz[i];
            index1=fea[2+i2*D];
            index2=fea[3+i2*D];
            fea[2+i2*D]=-1;
            fea[3+i2*D]=-1;
            fea[6+i2*D]=-1;
            if(index2>=0)
            {
                fea[2+index2*D]=-1;
                if(fea[3+index2*D]==-1)
                    fea[6+index2*D]=-1;
            }
        
            if(index1>=0)
            {
                fea[3+index1*D]=-1;
                if(fea[2+index1*D]==-1)
                    fea[6+index1*D]=-1;
            }
        }
    }
}

void slam_prune_fea(
        double *fea,
        int *ind2,
        int *mask,
        int N
         )
{
    int i,index1,index2,i2,D=14; 
    

    for(i=0;i<N;i++)    
    {
        
        if(mask[i]<0)
        {
           
            i2=ind2[i];
            index1=fea[2+i2*D];
            index2=fea[3+i2*D];
            fea[2+i2*D]=-1;
            fea[3+i2*D]=-1;
            fea[6+i2*D]=-1;
            if(index2>=0)
            {
                fea[2+index2*D]=-1;
                if(fea[3+index2*D]==-1)
                    fea[6+index2*D]=-1;
            }
        
            if(index1>=0)
            {
                fea[3+index1*D]=-1;
                if(fea[2+index1*D]==-1)
                    fea[6+index1*D]=-1;
            }
        }
    }
}
void slam_stereo_prune_fea(
        double *fea,
        int *ind2,
        int *mask,
        int N
         )
{
    int i,index,i2,D=14; 
    

    for(i=0;i<N;i++)    
    {
        
        if(mask[i]<0)
        {
           
            i2=ind2[i];
            if(i2<0)
            {
                printf("error: slam_stereo_prune_fea i2 is %d\n",i2);
                exit(1);
            }

            index=fea[7+i2*D];
            if(index>0)
            {
                fea[7+index*D]=-1;
            }
            fea[7+i2*D]=-1;
            
        }
    }
}

void slam_stereo_prune_slamfea(
        double *fea,
        int *indz,
        double *er,
        double th,
        int N
         )
{
    int i,index,next,prev,i2,D=14; 
    

    for(i=0;i<N;i++)    
    {
        
        if(er[i]>th)
        {           
            i2=indz[i];
            next=fea[2+i2*D];
            prev=fea[3+i2*D];
            
            fea[2+i2*D]=-1;
            fea[3+i2*D]=-1;
            fea[6+i2*D]=-1;
            fea[7+i2*D]=-1;
            
            index=fea[7+i2*D];
            if(index>=0)
              fea[7+index*D]=-1;
            if(prev>=0)
            {
                fea[2+prev*D]=next;
                if((fea[3+prev*D]==-1)&&next==-1)
                    fea[6+prev*D]=-1;
            }
        
            if(next>=0)
            {
                fea[3+next*D]=prev;
                if((fea[2+next*D]==-1)&&prev==-1)
                    fea[6+next*D]=-1;
            }
        }
    }
}

void init_fea(
		double *fea,
		double *features,
		int *frame,
		int *start,
		int N
	     )
{
    int i,D=10;
    
    start[0]=0;
    *frame=1;
    start[(*frame)]=N;

    for(i=0;i<N;i++)
    {
        fea[D*i]=features[3*i];
        fea[D*i+1]=features[3*i+1];
        fea[D*i+2]=-1;
        fea[D*i+3]=-1;
        fea[D*i+4]=features[3*i+2];
        fea[D*i+5]=(*frame);
        fea[D*i+6]=-1;
        fea[D*i+7]=-1;
        fea[D*i+8]=-1;
        fea[D*i+9]=-1;
    }
}

void stereo_init_fea(
        double *fea,
        double *features,
        int frame,
        int camera,
        int level,
        int *start,
        int N
         )
{
    int i,D=14;
    

    (*start)=N;

    for(i=0;i<N;i++)
    {
        fea[D*i]=features[3*i];
        fea[D*i+1]=features[3*i+1];
        fea[D*i+2]=-1;
        fea[D*i+3]=-1;
        fea[D*i+4]=camera;
        fea[D*i+5]=(frame);
        fea[D*i+6]=-1;
        fea[D*i+7]=-1;
        fea[D*i+8]=level;
        fea[D*i+9]=-1;
        fea[D*i+10]=-1;
        fea[D*i+11]=features[3*i+2];
        fea[D*i+12]=-1;
        fea[D*i+13]=-1;
    }
}
void stereo_add_fea(
        double *fea,
        double *featuresB,
        int frame,
        int camera,
        int level,
        int *start,
        int num_featuresB
         )
{
    int i,j,col,D=14; 
    printf("frame %d\n",frame);

    printf("num_featuresB %d\n",num_featuresB);
    for(i=0;i<num_featuresB;i++)
    {
       j=(*start)+i;
       col=D*j;
       fea[col]=featuresB[3*i];     /* x */
       fea[1+col]=featuresB[3*i+1];     /* y */
       fea[2+col]=-1;           /* next */
       fea[3+col]=-1;           /* prev */
       fea[4+col]=camera;           /* map */
       fea[5+col]=frame;        /* frame */
       fea[6+col]=-1;           /* map */
       fea[7+col]=-1;           /* other */
       fea[8+col]=level;           /* level */
       fea[9+col]=-1; 
       fea[10+col]=-1; 
       fea[11+col]=featuresB[3*i+2]; 
       fea[12+col]=-1; 
       fea[13+col]=-1; 
    }
    (*start)=(*start)+num_featuresB;
}


void slam_fea_update_matches(
        double *fea,
        int startA,
        int startB,
        int num_featuresA,
        int *matches,
        float *strength
         )
{
    int i,ind,D=14,cnt=0; 
    
    printf("num_featuresA %d\n",num_featuresA);
    for(i=0;i<num_featuresA;i++)    
    {
        ind=matches[i];
        if(ind>0)
        {
            cnt++;
            ind--;

            fea[2+(startA+i)*D]=startB+ind;      /* next*/
            fea[3+(startB+ind)*D]=startA+i;      /* prev */
            fea[6+D*(startB+ind)]=fea[6+D*(startA+i)];  /* map */
            fea[9+(startA+i)*D]=strength[2*i];      /* next*/
            fea[12+(startA+i)*D]=strength[2*i+1];      /* next*/
        }
    }
    printf("%d fea updated\n",cnt);
}
void slam_fea_update_stereo_matches(
        double *fea,
        int startA,
        int startB,
        int num_featuresA,
        int *matches,
        float *strength
         )
{
    int i,ind,D=14,cnt=0; 
    
    printf("num_featuresA %d\n",num_featuresA);
    for(i=0;i<num_featuresA;i++)    
    {
        ind=matches[i];
        if(ind>0)
        {
            cnt++;
            ind--;

            fea[7+(startA+i)*D]=startB+ind;      /* other*/
            fea[7+(startB+ind)*D]=startA+i;      /*other */
            fea[10+(startA+i)*D]=strength[2*i];      /* err*/
            if(fea[6+(startB+ind)*D]==-1)
              fea[6+(startB+ind)*D]=fea[6+(startA+i)*D];
            if(fea[6+(startA+i)*D]==-1)
              fea[6+(startA+i)*D]=fea[6+(startB+ind)*D];
       //     fea[6+D*(startB+ind)]=fea[6+D*(startA+i)];  /* map */
       // need to join features ? 
        }
    }
    printf("%d fea updated\n",cnt);
}


void parse_normal_fea_stereo(
        int *ind,
        int *indf,
        int *indc,
        double *e,
        double th,
        int *nind,
        int *jj,
        int w,
        int N
         )
{
    int i,j,numm,cnt,in;

    j=0;
    numm=ind[0];
    if((e[0]<th)&&(indc[0]==0)&&(indf[0]>=w))
    {
        nind[0]=0;
        cnt=1;
        in=1;
    }
    else
    {
        cnt=0;
        in=0;
    }
    
    for(i=1;i<N;i++)
    {
        if(numm==ind[i])
        {
            if(in)
            {
                if(e[i]<th&&indc[i]==0&&indf[i]>=0)
                {
                    cnt++;
                }
            }
        }
        else
        {
            if((cnt>=w)&&(cnt>=2)) /* if we have seen landmark more than (w-1) times increment j*/
                j++;
                      /* then the next candidate does not overwrite this*/
            if((e[i]<th)&&(indc[i]==0)&&(indf[i]>=w))
            {
                nind[j]=i;
                printf("nind: %d %d %d %d %d\n",j,i,nind[j],indf[i],w);
                cnt=1;
                in=1;
            }
            else
            {
                cnt=0;
                in=0;
            }
        }
        numm=ind[i];
    }
    *jj=j;
}
